#include <rclcpp/rclcpp.hpp>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <geometry_msgs/msg/twist_stamped.hpp>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <atomic>
#include <thread>
#include <rclcpp/executors/multi_threaded_executor.hpp>

class CombinedNode : public rclcpp::Node {
public:
    CombinedNode() : Node("combined_node"), force_sign(true), closest_index(0) {
        // 初始化MoveGroupInterface
        move_group_ = std::make_shared<moveit::planning_interface::MoveGroupInterface>(
            std::shared_ptr<rclcpp::Node>(this),  // 显式创建shared_ptr
            "zc4");
        
        RCLCPP_INFO(get_logger(), "节点初始化完成");
    }
    // 修改 start_execution 以接受路径索引
    void start_execution(int path_index) {
        std::thread([this, path_index]() {
            switch (path_index) {
                case 3:
                    executeCartesianPath3();
                    break;
                default:
                    RCLCPP_ERROR(get_logger(), "无效的路径索引: %d", path_index);
                    break;
            }
        }).detach();
    }

private:
    std::shared_ptr<moveit::planning_interface::MoveGroupInterface> move_group_;
    rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr force_sub_;
    std::atomic<bool> force_sign;
    std::atomic<size_t> closest_index;

    void force_callback(const geometry_msgs::msg::TwistStamped::SharedPtr msg) {
        if (std::abs(msg->twist.linear.z) >= 3.0) {
            force_sign = false;
        } else {
            force_sign = true;
        }
    }

    bool returnToMiddle() {
        move_group_->setNamedTarget("middle");
        moveit::planning_interface::MoveGroupInterface::Plan home_plan;
        bool success = (move_group_->plan(home_plan) == moveit::core::MoveItErrorCode::SUCCESS);
        if (success) {
            move_group_->execute(home_plan);
            RCLCPP_INFO(get_logger(), "回到middle位置");
            return true;
        }
        RCLCPP_ERROR(get_logger(), "回到middle位置失败");
        return false;
    }

    bool continueCartesianPath(const std::vector<geometry_msgs::msg::Pose>& original_waypoints) {
        geometry_msgs::msg::Pose current_pose = move_group_->getCurrentPose().pose;
        std::vector<geometry_msgs::msg::Pose> remaining_waypoints;
        double min_distance = std::numeric_limits<double>::max();

        for (size_t i = 0; i < original_waypoints.size(); ++i) {
            double dx = current_pose.position.x - original_waypoints[i].position.x;
            double dy = current_pose.position.y - original_waypoints[i].position.y;
            double dz = current_pose.position.z - original_waypoints[i].position.z;
            double distance = std::sqrt(dx*dx + dy*dy + dz*dz);
            if (distance < min_distance) {
                min_distance = distance;
                closest_index = i;
            }
        }

        for (size_t i = closest_index + 1; i < original_waypoints.size(); ++i) {
            remaining_waypoints.push_back(original_waypoints[i]);
        }

        if (remaining_waypoints.empty()) return false;

        move_group_->setStartStateToCurrentState();
        moveit::planning_interface::MoveGroupInterface::Plan new_plan;
        double fraction = move_group_->computeCartesianPath(remaining_waypoints, 0.01, 0.0, new_plan.trajectory_);
        
        if (fraction >= 0.9) {
            move_group_->asyncExecute(new_plan);
            return true;
        }
        return false;
    }

    void executeCartesianPath3() {
        if (!returnToMiddle()) return;
        /* #region  */
        std::vector<geometry_msgs::msg::Pose> waypoints;
        // 目标点0 28/-45/90/-45/0
        geometry_msgs::msg::Pose pose0;
        pose0.position.x = 0.083024;
        pose0.position.y = -0.10987;
        pose0.position.z = 0.28763;
        pose0.orientation.x = 0;
        pose0.orientation.y = 0;
        pose0.orientation.z = -0.24475;
        pose0.orientation.w = 0.96959;
        waypoints.push_back(pose0);

        // 目标点1 28/-50/90/15/0
        geometry_msgs::msg::Pose pose1;
        pose1.position.x = -0.034461;
        pose1.position.y = -0.046522;
        pose1.position.z = 0.10748;
        pose1.orientation.x = 0.11163;
        pose1.orientation.y = 0.44231;
        pose1.orientation.z = -0.21777;
        pose1.orientation.w = 0.86283;
        waypoints.push_back(pose1);

        // 目标点2 28/-60/100/10/0
        geometry_msgs::msg::Pose pose2;
        pose2.position.x = -0.033193;
        pose2.position.y = -0.047216;
        pose2.position.z = 0.096255;
        pose2.orientation.x = 0.10337;
        pose2.orientation.y = 0.40969;
        pose2.orientation.z = -0.22182;
        pose2.orientation.w = 0.87879;
        waypoints.push_back(pose2);

        // 目标点3 28/-65/110/0/0
        geometry_msgs::msg::Pose pose3;
        pose3.position.x = -0.016352;
        pose3.position.y = -0.056293;
        pose3.position.z = 0.087686;
        pose3.orientation.x = 0.093294;
        pose3.orientation.y = 0.36973;
        pose3.orientation.z = -0.22624;
        pose3.orientation.w = 0.89633;
        waypoints.push_back(pose3);

        // 目标点4 28/-70/119/-9/0
        geometry_msgs::msg::Pose pose4;
        pose4.position.x = -0.00072952;
        pose4.position.y = -0.064723;
        pose4.position.z = 0.083604;
        pose4.orientation.x = 0.082545;
        pose4.orientation.y = 0.32711;
        pose4.orientation.z = -0.23041;
        pose4.orientation.w = 0.91274;
        waypoints.push_back(pose4);

        // 目标点5 28/-78/134/-28/0 
        geometry_msgs::msg::Pose pose5;
        pose5.position.x = 0.030862;
        pose5.position.y = -0.081752;
        pose5.position.z = 0.082453;
        pose5.orientation.x = 0.059886;
        pose5.orientation.y = 0.23743;
        pose5.orientation.z = -0.23731;
        pose5.orientation.w = 0.94007;
        waypoints.push_back(pose5);

        // 目标点55 28/-78/136/-37/0 
        geometry_msgs::msg::Pose pose55;
        pose55.position.x = 0.045914;
        pose55.position.y = -0.08981;
        pose55.position.z = 0.10271;
        pose55.orientation.x = 0.045323;
        pose55.orientation.y = 0.17931;
        pose55.orientation.z = -0.24037;
        pose55.orientation.w = 0.9529;
        waypoints.push_back(pose55);

        // 目标点6 28/-78/138/-44/0
        geometry_msgs::msg::Pose pose6;
        pose6.position.x = 0.054854;
        pose6.position.y = -0.094613;
        pose6.position.z = 0.11793;
        pose6.orientation.x = 0.034817;
        pose6.orientation.y = 0.13775;
        pose6.orientation.z = -0.24206;
        pose6.orientation.w = 0.9598;
        waypoints.push_back(pose6);

        // 目标点7 28/-82/142/-52/0 半举平
        geometry_msgs::msg::Pose pose7;
        pose7.position.x = 0.061581;
        pose7.position.y = -0.098223;
        pose7.position.z = 0.14013;
        pose7.orientation.x = 0.017288;
        pose7.orientation.y = 0.068398;
        pose7.orientation.z = -0.24388;
        pose7.orientation.w = 0.96723;
        waypoints.push_back(pose7);

        // 目标点8 28/-82/145/-60/0 举平
        // 目标点8 28/-82/145/-58/0 举平
        geometry_msgs::msg::Pose pose8;
        pose8.position.x = 0.067408;
        pose8.position.y = -0.10135;
        pose8.position.z = 0.14599;
        pose8.orientation.x = 0.010242;
        pose8.orientation.y = 0.040522;
        pose8.orientation.z = -0.24425;
        pose8.orientation.w = 0.96881;
        waypoints.push_back(pose8);

        // 目标点8 28/-90/142/-50/0 刮板
        geometry_msgs::msg::Pose pose9;
        pose9.position.x = 0.053625;
        pose9.position.y = -0.093921;
        pose9.position.z = 0.16204;
        pose9.orientation.x = 0.0035733;
        pose9.orientation.y = 0.014151;
        pose9.orientation.z = -0.24446;
        pose9.orientation.w = 0.96955;
        waypoints.push_back(pose9);

        //第10个目标位姿 14/-90/142/-50/0 左移
        geometry_msgs::msg::Pose pose10;
        pose10.position.x = 0.071307;
        pose10.position.y = -0.047553;
        pose10.position.z = 0.16428;
        pose10.orientation.x = 0.0011452;
        pose10.orientation.y = 0.0093179;
        pose10.orientation.z = -0.12094;
        pose10.orientation.w = 0.99262;
        waypoints.push_back(pose10);

        //第11个目标位姿 0/-60/120/-60/0 low-middle
        geometry_msgs::msg::Pose pose11;
        pose11.position.x = 0.11027;
        pose11.position.y = 0;
        pose11.position.z = 0.226;
        pose11.orientation.x = 0;
        pose11.orientation.y = 0;
        pose11.orientation.z = 0;
        pose11.orientation.w = 1;
        waypoints.push_back(pose11);

    /* #endregion */

        move_group_->stop();
        move_group_->setStartStateToCurrentState();
        moveit::planning_interface::MoveGroupInterface::Plan cartesian_plan;
        double fraction = move_group_->computeCartesianPath(waypoints, 0.01, 0.0, cartesian_plan.trajectory_);
        
        if (fraction < 0.9) {
            RCLCPP_ERROR(get_logger(), "初始路径规划失败，完成度: %f", fraction);
            return;
        }

        RCLCPP_INFO(get_logger(), "初始路径规划成功，开始执行...");
        move_group_->asyncExecute(cartesian_plan);
        move_group_->stop();
        RCLCPP_INFO(get_logger(), "executeCartesianPath3 结束");
        rclcpp::shutdown();
    }

};

int main(int argc, char** argv) {
    rclcpp::init(argc, argv);
    auto node = std::make_shared<CombinedNode>();

    rclcpp::executors::MultiThreadedExecutor executor;
    executor.add_node(node);

    node->start_execution(3); 

    RCLCPP_INFO(node->get_logger(), "开始执行器 spin");
    executor.spin();

    RCLCPP_INFO(node->get_logger(), "执行器 spin 结束");
    rclcpp::shutdown();
    return 0;
}